#include "Tracking.h"


#define LEFT2 DL_GPIO_readPins(GPIO_Tracking_PORT, GPIO_Tracking_PIN_L2_PIN) 
#define LEFT1 DL_GPIO_readPins(GPIO_Tracking_PORT, GPIO_Tracking_PIN_L1_PIN) 
#define MID DL_GPIO_readPins(GPIO_Tracking_PORT, GPIO_Tracking_PIN_MID_PIN) 
#define RIGHT1 DL_GPIO_readPins(GPIO_Tracking_PORT, GPIO_Tracking_PIN_R1_PIN) 
#define RIGHT2 DL_GPIO_readPins(GPIO_Tracking_PORT, GPIO_Tracking_PIN_R2_PIN) 

/**
 * @brief   循迹模块获取误差err
 * @paraMID   None
 * @retval  None
 */
float Tracking_Err_Get(void)
{
    float err;
    if ((LEFT2 == GPIO_Tracking_PIN_L2_PIN) && (LEFT1 == 0) && (MID == 0) && (RIGHT1 == 0) && (RIGHT2 == 0)) // 10000 右边大偏
    {
        err = -2.5;   
    }

    else if ((LEFT2 == GPIO_Tracking_PIN_L2_PIN) && (LEFT1 == GPIO_Tracking_PIN_L1_PIN) && (MID == 0) && (RIGHT1 == 0) && (RIGHT2 == 0)) // 11000
    {
        err = -2;
    }

    else if ((LEFT2 == 0) && (LEFT1 == GPIO_Tracking_PIN_L1_PIN) && (MID == 0) && (RIGHT1 == 0) && (RIGHT2 == 0)) // 01000
    {
        err = -1.5;
    }

    else if ((LEFT2 == 0) && (LEFT1 == GPIO_Tracking_PIN_L1_PIN) && (MID == GPIO_Tracking_PIN_MID_PIN) && (RIGHT1 == 0) && (RIGHT2 == 0)) // 01100
    {
        err = -1;
    }

    else if ((LEFT2 == GPIO_Tracking_PIN_L2_PIN) && (LEFT1 == GPIO_Tracking_PIN_L1_PIN) && (MID == GPIO_Tracking_PIN_MID_PIN) && (RIGHT1 == 0) && (RIGHT2 == 0)) // 11100十字路口
    {
        err = -4;
    }

    else if ((LEFT2 == 0) && (LEFT1 == 0) && (MID == GPIO_Tracking_PIN_MID_PIN) && (RIGHT1 == 0) && (RIGHT2 == 0)) // 00100
    {
        err = 0;
    }

    else if ((LEFT2 == 0) && (LEFT1 == 0) && (MID == GPIO_Tracking_PIN_MID_PIN) && (RIGHT1 == GPIO_Tracking_PIN_R1_PIN) && (RIGHT2 == 0)) // 00110
    {
        err = 1;
    }

    else if ((LEFT2 == 0) && (LEFT1 == 0) && (MID == 0) && (RIGHT1 == GPIO_Tracking_PIN_R1_PIN) && (RIGHT2 == 0)) // 00010
    {
        err = 1.5;
    }

    else if ((LEFT2 == 0) && (LEFT1 == 0) && (MID == 0) && (RIGHT1 == GPIO_Tracking_PIN_R1_PIN) && (RIGHT2 == GPIO_Tracking_PIN_R2_PIN)) // 00011
    {
        err = 2;
    }

    else if ((LEFT2 == 0) && (LEFT1 == 0) && (MID == 0) && (RIGHT1 == 0) && (RIGHT2 == GPIO_Tracking_PIN_R2_PIN)) // 00001
    {
        err = 2.5;
    }

    else if ((LEFT2 == 0) && (LEFT1 == 0) && (MID == GPIO_Tracking_PIN_MID_PIN) && (RIGHT1 == GPIO_Tracking_PIN_R1_PIN) && (RIGHT2 == GPIO_Tracking_PIN_R2_PIN)) // 00111十字路口
    {
        err = 4;
    }

    // 十字路口

    else if ((LEFT2 == 0) && (LEFT1 == 0) && (MID == GPIO_Tracking_PIN_MID_PIN) && (RIGHT1 == 0) && (RIGHT2 == GPIO_Tracking_PIN_R2_PIN)) // 00101
    {
        err = 4;
    }

    else
        err = 0;

    return err;
}